/**
  ******************************************************************************
  * @file           
  * @author         古么宁
  * @brief          遥控相关处理
  ******************************************************************************
  *
  * COPYRIGHT(c) 2019 GoodMorning
  *
  ******************************************************************************
  */
/* Includes ---------------------------------------------------*/
#include <string.h>

#include "iec10x.h"
#include "iec10x_link.h"
#include "iec10x_telectrl.h" // telecontrol 
#include "iec10x_debug.h"

#include "../iec/devif.h"

#include "COT.h"
#include "ASDUTYP.h"

/* Private types ------------------------------------------------------------*/

struct iec10xslv_ctrl {
	uint8_t len;
	uint8_t ASDU[1]; /// 从
};


/// iec
struct iec10xmst_ctrl {
	ctrlcallback_t callbackfn   ;
	uint32_t       addr      ;
	uint32_t       value     ;
	uint8_t        type      ;
	uint8_t        cot       ;
};


enum iec101mst_ctrling {
	IEC_CTRL_NONE  = 0 ,
	IEC_CTRL_SELECT = 1 ,
	IEC_CTRL_SET   = 2 ,
	IEC_CTRL_END   = 3 ,
	IEC_CTRL_ERROR = 4 ,
};

/* Private macro ------------------------------------------------------------*/
/* Private variables --------------------------------------------------------*/
/* Private function prototypes -----------------------------------------------*/

//------------------------------------------------------------------------------


#if (!ENABLE_IEC10x_SLAVE)
	#define iec10xslv_telectrl(...)
	#define iec10xslv_set32bit(...)
#else


static void _iec101slv_ctrl_ack(iec10x_t * bus)
{
	iec101_protocol_send(bus,NULL,0,bus->iec101.lnctrl);
	if (bus->mode & (IEC_MASTER | IEC_BALANCE)) {
		if (bus->ctrl.todo != NULL) {
			bus->iec101.todo = bus->ctrl.todo;
		}
	}
}


static void iec101slv_ctrl_ack(iec10x_t * bus ,uint8_t ctrl , iec_fn_t nextfn)
{
	bus->iec101.lnctrl     = ctrl;
	bus->iec101.todo   = _iec101slv_ctrl_ack;
	bus->ctrl.todo = nextfn;
}


/**
  * @author   古么宁
  * @brief    从机遥控响应发送函数
  * @param    bus      : 总线
  * @return   don't care
*/
static void iec10xslv_ctrlreply(iec10x_t * bus)
{
	unsigned char lnkctrl;
	struct iec10xslv_ctrl * ctrlpkt ;
	ctrlpkt = (struct iec10xslv_ctrl *)&bus->ctrl.argv[0];

	if (!(bus->mode & IEC_101)) {
		/* iec104 */
		iec104_protocol_send(bus,ctrlpkt->ASDU,ctrlpkt->len,0);
	}
	else {
		/* iec101 */
		if (bus->mode & IEC_BALANCE) { 
			uint8_t FCB = (bus->iec101.FCB== 0) ;
			bus->iec101.FCB= FCB;
			lnkctrl  = IEC_CTRL(0,1,FCB,1,MASTER_TRANS);
			lnkctrl |= (bus->mode & IEC_DIRECT);
		}
		else 
		if (bus->ctrling == 2){
			lnkctrl = IEC_CTRL(0,0,1,0,SLAVE_OK);
		}
		else {
			lnkctrl = IEC_CTRL(0,0,0,0,SLAVE_OK);
		}
		iec101_protocol_send(bus,ctrlpkt->ASDU,ctrlpkt->len,lnkctrl);
	}


	if (bus->ctrling == IEC_CTRL_SET || bus->ctrling == IEC_CTRL_ERROR) {
		bus->ctrling = 0;
	}
	
	memset(&bus->ctrl,0,sizeof(struct iec_do));
	//bus->ctrl.todo = NULL;
}



/**
  * @author   古么宁
  * @brief    从机遥控响应
  * @param    bus      : 总线
  * @param    ASDUrx   : 接收到的 ASDU 数据包
  * @param    ASDUlen  : 接收到的 ASDU 数据包长度
  * @return   don't care
*/
static void iec10xslv_telectrl(iec10x_t * bus , uint8_t * ASDUrx,int ASDUrxlen)
{
	IEC_INDEX(bus->causelen,bus->cmnaddrlen);
	uint8_t COT ,*ctrlinfo , ctrlcmd ,lnkctrl ;
	uint32_t  ctrladdr = 0 ;

	struct iec_devif *      dev     ;
	struct iec10xslv_ctrl * ctrlpkt ;

	bus->ctrlt = iec_timestamp;
	dev = iec_devif_find(&bus->device,ASDUrx[IEC_TYPE]);
	
	COT = ASDUrx[IEC_COT] ;
	ctrlinfo = &ASDUrx[IEC_INFO + bus->infoaddrlen];
	
	ctrlpkt = (struct iec10xslv_ctrl *)bus->ctrl.argv;
	ctrlpkt->len = ASDUrxlen ;
	memcpy(ctrlpkt->ASDU,ASDUrx,ASDUrxlen);
	
	/* 填充地址 */
	for (int i = 0 ; i < bus->infoaddrlen ; ++i) {
		ctrladdr |= (ASDUrx[IEC_INFO + i] << (i*8)) ; 
	}

	/* 根据是否带时标，摘取控制命令 */
	if (ASDUrx[IEC_TYPE] < C_SC_TA_1) {
		ctrlcmd = ASDUrx[ASDUrxlen - 1] ;
	}
	else {
		ctrlcmd = ASDUrx[ASDUrxlen - 1 - 7] ;
	}

	if (ctrlcmd & 0x80) {
		/* 主站发送选择激活命令，如果当前遥控状态不为空则返回错误 */
		if (bus->ctrling == 0){
			bus->ctrling = IEC_CTRL_SELECT ;
		}
		else {
			bus->ctrling = IEC_CTRL_ERROR;
			ctrlpkt->ASDU[IEC_COT] = COT_ACTIVATE_EXIT;
			goto send;
		}
	}
	else 
	if (IEC_CTRL_SELECT == bus->ctrling) {
		/* 主站发送执行命令，且当前遥控点已被选择，执行遥控 */
		bus->ctrling = IEC_CTRL_SET ;
	}
	else {
		bus->ctrling = IEC_CTRL_ERROR;
		ctrlpkt->ASDU[IEC_COT] = COT_ACTIVATE_EXIT;
		iec10x_warn(bus,"ctrl without select.\r\n");
		goto send;
	}

	/* 判断是否为遥控撤销 */
	if (ASDUrx[IEC_COT] == COT_ACTIVATE_STOP) {
		ctrlpkt->ASDU[IEC_COT] = ASDUrx[IEC_COT] + 1;
	}
	else
	if (!dev || !dev->profn) {
		/* 如果应用层没有当前遥控类型对应的接口，返回结束 */
		bus->ctrling = IEC_CTRL_ERROR ;
		ctrlpkt->ASDU[IEC_COT] = COT_ACTIVATE_EXIT;
		iec10x_warn(bus,"user device interface not found\r\n");
	}
	else 
	if (0 == dev->profn(bus,COT,1,&ctrladdr,ctrlinfo)) {
		ctrlpkt->ASDU[IEC_COT] = ASDUrx[IEC_COT] + 1;
	}
	else {
		bus->ctrling = IEC_CTRL_ERROR;
		ctrlpkt->ASDU[IEC_COT] = COT_ACTIVATE_EXIT;
	}

send:
	/* 使能发送 */
	if (!(bus->mode & IEC_101)) {
		bus->ctrl.todo = iec10xslv_ctrlreply ;
	}
	else {
		/* iec101，先回复短帧确认，再回复数据确认帧 */
		if (bus->mode & IEC_BALANCE) {
			lnkctrl  = IEC_CTRL(0,0,0,0,SLAVE_OK);
			lnkctrl |= (bus->mode & IEC_DIRECT);
			iec101_protocol_send(bus,NULL,0,lnkctrl);
			//bus->ctrl.todo = iec10xslv_ctrlreply;
			bus->iec101.todo = bus->ctrl.todo;
		}
		else {
			lnkctrl = SLVCTRL(SLAVE_OK,0,1);
			iec101_protocol_send(bus,NULL,0,lnkctrl);
			bus->ctrl.todo = iec10xslv_ctrlreply;
		}
	}
}



/**
  * @author   古么宁
  * @brief    设置 32 bit 串
  * @param    bus      : 总线
  * @param    ASDUrx   : 接收到的 ASDU 数据包
  * @param    ASDUlen  : 接收到的 ASDU 数据包长度
  * @return   don't care
*/
static void iec10xslv_set32bit(iec10x_t * bus , uint8_t * ASDUrx,int ASDUrxlen)
{
	IEC_INDEX(bus->causelen,bus->cmnaddrlen);
	uint32_t  ctrladdr = 0 ;
	uint8_t   COT          ;
	uint8_t * data = &ASDUrx[IEC_INFO + bus->infoaddrlen];

	struct iec10xslv_ctrl * ctrlpkt ;
	struct iec_devif *      dev;

	for (int i = 0 ; i < bus->infoaddrlen ; ++i) {// 填充信息对象地址
		ctrladdr  |= (ASDUrx[IEC_INFO + i] << (8*i)) ; 
	}
	iec10x_msg(bus,"get 32 bit\r\n");

	dev = iec_devif_find(&bus->device,ASDUrx[IEC_TYPE]);
	COT = COT_ACTIVATE_EXIT ;
	
	if (!dev || !dev->profn) {
		iec10x_warn(bus,"set 32bits:device interface not found!\r\n");
	}
	else
	if (0 != dev->profn(bus,ASDUrx[IEC_COT],1,&ctrladdr,data)) {
		iec10x_warn(bus,"set 32bits!\r\n");
	}
	else {
		COT = COT_ACTIVATE_CONFIRM;
	}

	ctrlpkt = (struct iec10xslv_ctrl *)bus->ctrl.argv;
	ctrlpkt->len = ASDUrxlen ;
	memcpy(ctrlpkt->ASDU,ASDUrx,ASDUrxlen);
	ctrlpkt->ASDU[IEC_COT] = COT;
	
	bus->ctrling = IEC_CTRL_SET;
	
	/* 使能发送 */
	if (!(bus->mode & IEC_101)) {
		bus->ctrl.todo = iec10xslv_ctrlreply ;
	}
	else {
		uint8_t lnkctrl ;
		if (bus->mode & IEC_BALANCE) {
			lnkctrl  = IEC_CTRL(0,0,0,0,SLAVE_OK);
			lnkctrl |= (bus->mode & IEC_DIRECT);
		}
		else {
			lnkctrl = SLVCTRL(SLAVE_OK,0,1);
		}
		iec101slv_ctrl_ack(bus,lnkctrl,iec10xslv_ctrlreply);
	}
}

#endif /* #if (!ENABLE_IEC10x_SLAVE) */


// ---------------------------------- MASTER ----------------------------------

#if (!ENABLE_IEC10x_MASTER)

	/* 不使能主站功能时 */
	#define iec10xmst_telectrl(...)
	#define iec10xmst_set32bit(...)
	void    iec10xmst_telectrl_req(
		iec10x_t *     bus         ,
		telectrl_t     ctrltype    ,
		uint32_t       ctrladdr    ,
		uint32_t       ctrlval     , 
		ctrlcallback_t callbackfn ) {}

#else


static void iec10xmst_ctrl_pkt(iec10x_t * bus)
{
	IEC_INDEX(bus->causelen,bus->cmnaddrlen);
	uint8_t   QOS ;
	uint8_t * ASDU = ASDU_TXBUFF(bus);
	uint8_t * info = &ASDU[IEC_INFO + bus->infoaddrlen];

	struct iec10xmst_ctrl * ctrl ;

	ctrl = (struct iec10xmst_ctrl *)bus->ctrl.argv;
	
	ASDU[IEC_TYPE]          = ctrl->type              ;
	ASDU[IEC_QUALIFIER]     = 0x01                    ;
	ASDU[IEC_COT]           = ctrl->cot               ;
	ASDU[IEC_COT+1]         = bus->connectaddr & 0xff ;
	ASDU[IEC_COMMON_ADDR]   = bus->cmnaddr     & 0xff ;
	ASDU[IEC_COMMON_ADDR+1] = bus->cmnaddr     >> 8   ;
	ASDU[IEC_INFO]          = (ctrl->addr >> 0) & 0xff;
	ASDU[IEC_INFO+1]        = (ctrl->addr >> 8) & 0xff;
	ASDU[IEC_INFO+2]        = (ctrl->addr >>16) & 0xff;

	if (!bus->ctrling) {
		QOS = 0x80 ;
		bus->ctrling = IEC_CTRL_SELECT ;
	}
	else {
		QOS = 0x00 ;
		bus->ctrling = IEC_CTRL_SET ;
	}

	switch (ctrl->type) {
	case SIQ_SET :
	case DIQ_SET :
	case VTI_SET :
		*info++ = (QOS|ctrl->value) ;
		break;
	case NVA_SET :
	case SVA_SET :
		*info++ = ctrl->value & 0x00ff ;
		*info++ = (ctrl->value>>8) & 0x00ff ;
		*info++ = QOS ; // QOS
		break;
	case STD_SET :
		*info++ = ctrl->value & 0x00ff ;
		*info++ = (ctrl->value>>8) & 0x00ff ;
		*info++ = (ctrl->value>>16) & 0x00ff ;
		*info++ = (ctrl->value>>24) & 0x00ff ;
		*info++ = QOS ; // QOS
		break;
	case S32_BIT:
		*info++ = ctrl->value & 0x00ff ;
		*info++ = (ctrl->value>>8) & 0x00ff ;
		*info++ = (ctrl->value>>16) & 0x00ff ;
		*info++ = (ctrl->value>>24) & 0x00ff ;
		break;
	default: return ;
	}


	bus->ctrl.todo = NULL;
	bus->busy |= IEC10x_CTRLING ;
	
	if (!(bus->mode & IEC_101)) {
		iec104_protocol_send(bus,ASDU,info - ASDU,0);
	}
	else {
		uint8_t lnkctrl ;
		if (bus->mode & IEC_BALANCE) {
			int FCB = (bus->iec101.FCB== 0) ;
			bus->iec101.FCB= FCB;
			lnkctrl  = IEC_CTRL(0,1,FCB,1,MASTER_TRANS);
			lnkctrl |= (bus->mode & IEC_DIRECT);
		}
		else {
			lnkctrl = MSTCTRL(MASTER_TRANS,1,0);
		}
		iec101_protocol_send(bus,ASDU,info - ASDU,lnkctrl);
	}
}

/**
  * @author   古么宁
  * @brief    主站退出遥控流程
  * @param    bus      : 总线
  * @param    result   : 从机端的遥控结果
  * @return   don't care
*/
static void iec10xmst_ctrl_exit(iec10x_t * bus , int result)
{
	struct iec10xmst_ctrl * ctrl ;
	ctrl = (struct iec10xmst_ctrl *)bus->ctrl.argv;
	if (ctrl->callbackfn)
		ctrl->callbackfn(bus,ctrl->type,ctrl->addr,ctrl->value,result);
	bus->ctrling = 0;
	bus->busy   &= ~IEC10x_CTRLING;
	memset(&bus->ctrl,0,sizeof(struct iec_do));
}


/**
  * @author   古么宁
  * @brief    主站遥控响应，收到从站的回复确认帧
  * @param    bus      : 总线
  * @param    ASDUrx   : 接收到的 ASDU 数据包
  * @param    ASDUlen  : 接收到的 ASDU 数据包长度
  * @return   don't care
*/
static void iec10xmst_telectrl(iec10x_t * bus , uint8_t * ASDUrx,int ASDUrxlen)
{
	IEC_INDEX(bus->causelen,bus->cmnaddrlen);
	int       COT      = ASDUrx[IEC_COT] ;
	int       ctrlcmd  = ASDUrx[ASDUrxlen-1];
	uint8_t * info     = &ASDUrx[IEC_INFO] ;
	uint32_t  ctrladdr = 0 ;
	
	struct iec10xmst_ctrl * ctrl ;

	ctrl = (struct iec10xmst_ctrl *)bus->ctrl.argv;

	/* 合并起始的信息对象地址 */
	for (int k = 0 ; k < bus->infoaddrlen ; ++k , ++info) {
		ctrladdr |= (*info << (k*8));
	}

	if (ctrladdr != ctrl->addr) {
		/* 地址错误，退出 */
		iec10xmst_ctrl_exit(bus,CTRL_ERROR);
	}
	else
	if (COT_ACTIVATE_CONFIRM == COT) {
		if (ctrlcmd & 0x80) { 
			/* 从机返回选择激活确认，主站下一帧发送遥控执行包 
			   在 iec10x_periodic_handle() 执行 iec10xmst_ctrl_pkt() */
			bus->ctrl.todo = iec10xmst_ctrl_pkt;
		}
		else {
			/* 执行包激活确认，流程结束 */
			iec10xmst_ctrl_exit(bus,CTRL_SUCCESS);
		}
	}
	else 
	if (COT_ACTIVATE_EXIT == COT ) {
		/* 子站返回激活结束否认当前遥控 */
		iec10xmst_ctrl_exit(bus,CTRL_ERROR);
	}
	else 
	if (COT_UNKNOWN_OBJ_ADDR == COT) {
		iec10xmst_ctrl_exit(bus,CTRLADDR_ERR);
	}
}


/**
  * @author   古么宁
  * @brief    设置 32 bit 串
  * @param    bus      : 总线
  * @param    ASDUrx   : 接收到的 ASDU 数据包
  * @param    ASDUlen  : 接收到的 ASDU 数据包长度
  * @return   don't care
*/
void iec10xmst_set32bit(iec10x_t * bus , uint8_t * ASDUrx,int ASDUrxlen)
{
	IEC_INDEX(bus->causelen,bus->cmnaddrlen);
	uint8_t   COT = ASDUrx[IEC_COT];
	uint8_t  *val = &ASDUrx[IEC_INFO+ bus->infoaddrlen];
	uint32_t  ctrladdr = 0 ;
	uint32_t  ctrlval  = 0 ;
	struct iec10xmst_ctrl * ctrl ;
	ctrl = (struct iec10xmst_ctrl *)bus->ctrl.argv;
	bus->busy   &= ~IEC10x_CTRLING ;
	bus->ctrling = 0 ;

	/* 填充地址 */
	for (int i = 0 ; i < bus->infoaddrlen ; ++i) {
		ctrladdr |= (ASDUrx[IEC_INFO + i] << (i*8)) ; 
	}
	
	ctrlval = val[0] | (val[1]<<8)|(val[2]<<16)|(val[3]<<24);

	if (COT == COT_ACTIVATE_CONFIRM) {
		iec10x_msg(bus,"set 32 bit successfully!\r\n");
		if (ctrl->callbackfn) {
			ctrl->callbackfn(bus,S32_BIT,ctrladdr,ctrlval,CTRL_SUCCESS);
		}
	}
	else {
		iec10x_warn(bus,"set 32 bit failed!\r\n");
		if (ctrl->callbackfn) {
			ctrl->callbackfn(bus,S32_BIT,ctrladdr,ctrlval,CTRL_ERROR);
		}
	}
	memset(bus->ctrl.argv,0,sizeof(struct iec_do));
}


/**
  * @brief    主站发送一个遥控请求
  * @param    bus         : 总线
  * @param    ctrladdr    : 遥控结构体对象所在地址
  * @param    ctrltype    : 遥控类型，参考 @see telectrl_t 
  * @param    ctrlval     : 遥控值，即设置 ctrladdr 为什么值
  * @param    callbackfn  : 遥控成功回调，遥控成功后执行，可以为 NULL ，详细定义参考 @see ctrlcallback_t
  * @return   don't care
*/
void iec10xmst_telectrl_req(
	iec10x_t *     bus         ,
	telectrl_t     ctrltype    ,
	uint32_t       ctrladdr    ,
	uint32_t       ctrlval     , 
	ctrlcallback_t callbackfn )
{
	if (!bus->connected || !(bus->mode & IEC_MASTER)) {
		if (callbackfn)
			callbackfn(bus,ctrltype,ctrladdr,ctrlval,CTRL_ERROR);
		return ;
	} 

	if (bus->ctrling) {
		iec10x_err(bus,"controling...\r\n");
		if (callbackfn)
			callbackfn(bus,ctrltype,ctrladdr,ctrlval,CTRL_ERROR);
		return ;
	}

	struct iec10xmst_ctrl *newctrl ;

	newctrl = (struct iec10xmst_ctrl*)&bus->ctrl.argv;
	newctrl->callbackfn  = callbackfn         ;
	newctrl->value       = ctrlval            ;  
	newctrl->type        = ctrltype           ;
	newctrl->addr        = ctrladdr           ;
	newctrl->cot         = COT_ACTIVATE       ;

	/* 在 iec10x_periodic_handle() 执行 iec10xmst_ctrl_pkt() */
	bus->ctrl.todo       = iec10xmst_ctrl_pkt ;
}

#endif /** #if (!ENABLE_IEC10x_MASTER) */

/**
  * @author   古么宁
  * @brief    设置 32 bit 串
  * @param    bus      : 总线
  * @param    ASDUrx   : 接收到的 ASDU 数据包
  * @param    ASDUlen  : 接收到的 ASDU 数据包长度
  * @return   don't care
*/
void iec10x_set32bit(iec10x_t * bus , uint8_t * ASDUrx,int ASDUrxlen)
{
	if (bus->mode & IEC_MASTER) {
		iec10xmst_set32bit(bus,ASDUrx,ASDUrxlen);
	}
	else {
		iec10xslv_set32bit(bus,ASDUrx,ASDUrxlen);
	}
}



/**
  * @author   古么宁
  * @brief    遥控响应
  * @param    bus      : 总线
  * @param    ASDUrx   : 接收到的 ASDU 数据包
  * @param    ASDUlen  : 接收到的 ASDU 数据包长度
  * @return   don't care
*/
void iec10x_telectrl_respond(iec10x_t * bus , uint8_t * ASDUrx,int ASDUrxlen)
{
	if (bus->mode & IEC_MASTER) {
		iec10xmst_telectrl(bus,ASDUrx,ASDUrxlen);
	}
	else {
		iec10xslv_telectrl(bus,ASDUrx,ASDUrxlen);
	}
}
